//
// Created by chizuru on 2025/9/29.
//

#include "SensorTPDevice.h"
#include "FunctionResponseTerminator.h"
#include "modbuserr.h"
#include <cassert>
using std::cerr;
using std::hex;
SensorTPDevice::SensorTPDevice(DeviceComm &comm, std::unique_ptr<IProtocolCodec> codec): comm(comm), codec(std::move(codec)) { }

std::string printModbusCodecErrorMessage(uint8_t errorCode) {
    switch (0x0f & errorCode) {
        case 0x01: return "CRC error.";
        case 0x02: return "length / byte count mismatch.";
        case 0x03: return "function code mismatch.";
        case 0x04: return "parse error.";
        case 0x06: return "the response content does not match the expected content.";
        default: return "Unknown error";
    }
}
std::vector<std::optional<uint32_t>>
SensorTPDevice::readRegisters(uint8_t dev, uint16_t addr, const std::vector<uint16_t>& registerCounts) {
    return retryPolicy.withRetry([&]() -> std::vector<std::optional<uint32_t>>{
        IProtocolCodec::CodecArgs args;
        args.push_back(dev);
        args.push_back(addr);
        uint16_t sumOfRegCounts = 0;
        std::for_each(registerCounts.begin(), registerCounts.end(), [&](uint16_t regCount){
            sumOfRegCounts += regCount;
        });
        args.push_back(sumOfRegCounts);
        FunctionResponseTerminator term([&](const std::vector<uint8_t>& buf){
            if (buf.size() < 2)
                return false;
            if (buf[1] & 0x80)
                return buf.size() >=  5;
            return buf.size() >= (sumOfRegCounts * 2 + 5);
        });
        auto frame = codec->buildReadFrame(uint8_t(0x03), args);
        std::vector<uint8_t> resp;
        auto opt = comm.sendFrameAndReadResponse(frame,
                                                 term,
                                                 resp,
                                                 SERIAL_COMMAND_TRANSMIT_TIME,
                                                 SERIAL_COMMAND_TIMEOUT,
                                                 SERIAL_COMMAND_INTER_READ_DELAY);

        if (!opt.success) {
            return {};
        }
        IProtocolCodec::CodecArgs decodeArgs(registerCounts.begin(), registerCounts.end());
        ProtocolResult pr = codec->parseReadResponse(resp, uint8_t(0x03), decodeArgs);
        if (!pr.ok) {
            if (pr.errorCode & 0x80)
                cerr << "Read register 0x" << hex << addr << " NACK " << hex << int(0x0f & pr.errorCode) << " (" << printModbusErrorMessage(pr.errorCode)  << ")" <<"\n";
            else
                cerr << "Read register 0x" << hex << addr << " failed: "<< printModbusCodecErrorMessage(pr.errorCode) << "\n";
            return {};
        }

        if (pr.values.size() == registerCounts.size()) {
            std::vector<std::optional<uint32_t>> parameters;
            uint32_t *pi = nullptr;
            for (size_t i = 0; i < registerCounts.size() && (pi = std::get_if<uint32_t>(&pr.values[i])); i++) {
                parameters.push_back(std::optional<uint32_t>{*pi});
            }
            if (parameters.size() != registerCounts.size()) {
                return  {};
            }

            return parameters;
        }

        return {};
    });
}

bool SensorTPDevice::writeRegisters(uint8_t dev, uint16_t addr, uint16_t registerCount,
                                    const std::vector<uint16_t> &values) {
    return retryPolicy.withRetry([&]() -> bool{
        FunctionResponseTerminator term([&](const std::vector<uint8_t>& buf){
            if (buf.size() < 2)
                return false;
            if (buf[1] & 0x80)
                return buf.size() >=  5;
            return buf.size() >= 8;
        });
        auto buildVerificationFrame = [&]() -> uint32_t {
            if (registerCount == 1 && values.size() == 1) {
                uint32_t ver = (static_cast<uint32_t>(addr) << 16 ) | values[0];
                return ver;
            } else if (registerCount > 1 && registerCount == values.size() ) {
                uint32_t ver = (static_cast<uint32_t>(addr) << 16) | registerCount;
                return ver;
            }
            return 0x3f3f3f3f;
        };
        if (registerCount == 1 && values.size() == 1) {
            IProtocolCodec::CodecArgs args;
            args.push_back(dev);
            args.push_back(addr);
            args.push_back(registerCount);
            args.push_back(values[0]);
            auto frame = codec->buildWriteFrame(uint8_t(0x06), args);

            std::vector<uint8_t> resp;
            auto opt = comm.sendFrameAndReadResponse(frame,
                                                     term,
                                                     resp,
                                                     SERIAL_COMMAND_TRANSMIT_TIME,
                                                     SERIAL_COMMAND_TIMEOUT,
                                                     SERIAL_COMMAND_INTER_READ_DELAY);

            if (!opt.success) {
                return false;
            }
            IProtocolCodec::CodecArgs decodeArgs;
            auto var = buildVerificationFrame();
            assert(var != 0x3f3f3f3f);
            decodeArgs.push_back(var);
            ProtocolResult pr = codec->parseWriteResponse(resp, uint8_t(0x06), decodeArgs);
            if (!pr.ok) {
                if (pr.errorCode & 0x80)
                    cerr << "Write register 0x" << hex << addr << " failed: "<< printModbusErrorMessage(pr.errorCode) << "\n";
                else
                    cerr << "Write register 0x" << hex << addr << " failed: "<< printModbusCodecErrorMessage(pr.errorCode) << "\n";
                return false;
            }
            return true;
        }else if (registerCount > 1 && registerCount == values.size() ) {
            IProtocolCodec::CodecArgs args;
            args.push_back(dev);
            args.push_back(addr);
            args.push_back(registerCount);
            args.insert(args.end(), values.begin(), values.end());
            auto frame = codec->buildWriteFrame(uint8_t(0x10), args);

            std::vector<uint8_t> resp;
            auto opt = comm.sendFrameAndReadResponse(frame,
                                                     term,
                                                     resp,
                                                     SERIAL_COMMAND_TRANSMIT_TIME,
                                                     SERIAL_COMMAND_TIMEOUT,
                                                     SERIAL_COMMAND_INTER_READ_DELAY);

            if (!opt.success) {
                return false;
            }
            IProtocolCodec::CodecArgs decodeArgs;
            auto var = buildVerificationFrame();
            assert(var != 0x3f3f3f3f);
            decodeArgs.push_back(var);
            ProtocolResult pr = codec->parseWriteResponse(resp, uint8_t(0x10), decodeArgs);
            if (!pr.ok) {
                if (pr.errorCode & 0x80)
                    cerr << "Write register 0x" << hex << addr << " failed: "<< printModbusErrorMessage(pr.errorCode) << "\n";
                else
                    cerr << "Write register 0x" << hex << addr << " failed: "<< printModbusCodecErrorMessage(pr.errorCode) << "\n";
                return false;
            }
            return true;
        }
        return false;
    });
}

void SensorTPDevice::setRetryPolicy(const RetryPolicy &rp) { retryPolicy = rp; }

